package regulators;
import java.util.ArrayList;

import gui.OpCom;
import monitor.ControllerStatMonitor;
import monitor.ExcecutionMonitor;
import monitor.StateVariableMonitor;
import monitor.TrackingMonitor;
import se.lth.control.realtime.*;
import stateMachine.StateMachine.Ball;
import controllers.PID;
import controllers.PIDParameters;

public class BallAndBeamRegul extends Thread implements Regul {

	// Declarations
	public PIDParameters outerContParameters;
	public PIDParameters innerContParameters;
	private AnalogIn analogInAngle; 
	private AnalogIn analogInPosition; 
	private AnalogOut analogOut;
	private TrackingMonitor trackingMonitor;
	StateVariableMonitor stateVariableMonitor;
	public ControllerStatMonitor controllerStatMonitor;
	private boolean tracking;
	private Ball ballSize;
	private double ref;
	private PID pid_inner_ctrl;
	private PID pid_outer_ctrl;
	private double u;
	double angle;
	double angle_ref;

	public static ExcecutionMonitor excecutionMonitor;

	public BallAndBeamRegul(AnalogIn analogInPosition,AnalogIn analogInAngle, AnalogOut analogOut1, double initRef, int pri, OpCom op, StateVariableMonitor stateVariableMonitor, ControllerStatMonitor controllerStatMonitor) {
		pid_inner_ctrl = new PID("PID");
		pid_outer_ctrl = new PID("PID");
		ref = initRef;
		outerContParameters = new PIDParameters();
		outerContParameters.K = -0.2;
		outerContParameters.Ti = 12;
		outerContParameters.Tr = 7;
		outerContParameters.Td = 0.9;
		outerContParameters.Beta = 1;
		outerContParameters.H = 0.01;
		outerContParameters.N = 5.0;
		outerContParameters.integratorOn = true;
		setOuterParameters(outerContParameters);

		innerContParameters = new PIDParameters();
		innerContParameters.K = 3.5;
		innerContParameters.Ti = 0.25;
		innerContParameters.Tr = 7;
		innerContParameters.Beta = 0.8;
		innerContParameters.H = 0.01;
		innerContParameters.Td = 0.25;
		innerContParameters.N = 5.0;
		innerContParameters.integratorOn = true;
		setInnerParameters(innerContParameters);
		this.controllerStatMonitor = controllerStatMonitor;
		trackingMonitor = new TrackingMonitor();
		excecutionMonitor = new ExcecutionMonitor();
		excecutionMonitor.setRunning(false);
		this.stateVariableMonitor = stateVariableMonitor;
		tracking = false;
		this.analogInPosition = analogInPosition; 
		this.analogInAngle = analogInAngle; 
		analogOut = analogOut1;		

		setPriority(pri);
	}

	private double outer_max=10, outer_min=-10;
	private double limit_outer(double u) {
		if (u < outer_min) {
			u = outer_min;
		} else if (u > outer_max) {
			u = outer_max;
		} 
		return u;
	}

	private double inner_max=10, inner_min=-10;
	private double limit_inner(double u) {
		if (u < inner_min) {
			u = inner_min;
		} else if (u > inner_max) {
			u = inner_max;
		} 
		return u;
	}


	@Override
	public void run() {
		double pos = 0;
		double angle = 0;
		long t;
		while(true){
			
				t = System.currentTimeMillis();

				try {
					pos = analogInPosition.get();
					controllerStatMonitor.setBallPosition1(pos);
				} catch (IOChannelException e1) {
					e1.printStackTrace();
				}

				try {
					angle = analogInAngle.get();
					controllerStatMonitor.setBeamAngle1(angle);
				} catch (IOChannelException e1) {
					e1.printStackTrace();
				}



				/* outer loop */
				synchronized (pid_outer_ctrl) {

					if(tracking){
						ref = trackingMonitor.getRef();
					}
					angle_ref = limit_outer(pid_outer_ctrl.calculateOutput(pos, ref));

					pid_outer_ctrl.updateState(angle_ref);
				}

				/* inner loop */
				synchronized (pid_inner_ctrl) { 

					u = limit_inner(pid_inner_ctrl.calculateOutput(angle, angle_ref));
					
					if(isRunning()) {
					try {
						analogOut.set(u);
						controllerStatMonitor.setControlSignal(u);
					} catch (IOChannelException e) {
						e.printStackTrace();
					}
					}


					pid_inner_ctrl.updateState(u);

					
				}
				if(tracking){
					trackingMonitor.addUValue(u);
					if(trackingMonitor.trackingArrayFilled()){
						tracking = false;
						ballSize = trackingMonitor.determineBallSize();
						
					}
				}



				t = t + pid_inner_ctrl.getHMillis();
				long duration = t - System.currentTimeMillis();
				if (duration > 0) {
					try {
						sleep(duration);
					} catch (InterruptedException e) {
						e.printStackTrace();
					}
				}
			}
		}



	public void BallTrack(boolean b) {
		tracking = b;

	}


	public void shutDown() {
		excecutionMonitor.setRunning(false);
		setRef(0.0);

	}


	public void restart() {
		pid_inner_ctrl.reset();
		pid_outer_ctrl.reset();
		excecutionMonitor.setRunning(true);
	}


	public void setRef(double reference) {
		controllerStatMonitor.setRef(reference);
		ref = reference;
		
		
	}


	public boolean isRunning() {
		return excecutionMonitor.isRunning();
	}


	public Ball getBallSize() {
		return ballSize;
	}
	
	public void resetBallSize() {
		ballSize = null;
	}


	@Override
	public synchronized PIDParameters getInnerParameters() {
		return pid_inner_ctrl.getParameters();
	}


	@Override
	public synchronized PIDParameters getOuterParameters() {
		return pid_outer_ctrl.getParameters();
	}


	@Override
	public synchronized void setInnerParameters(PIDParameters innerPar) {
		pid_inner_ctrl.setParameters(innerPar);

	}


	@Override
	public synchronized void setOuterParameters(PIDParameters outerPar) {
		pid_outer_ctrl.setParameters(outerPar);

	}

	public double getRef() {
		return ref;
	}
	
	public ArrayList<double[]> getTracking(){
		return trackingMonitor.getWeightM();
	}
	
	public ArrayList<Double> getMean(){
		return trackingMonitor.getMean();
	}
	
}

